#include "common.h"
#include "lift.h"

static struct {
	int PWM;
	int pos;
	int basePos;		// the pot value at the lower limit switch
	int target;
	int timeout;
	
	// Stall protection variables
	int stallPos;
	int stallTime;
	int coolTime;
	char locked;		// Stall protection activated, and someone is still trying to apply power. Will not unlock until power is removed.
} sLift = {0, 0, 44, -1, 0, 0, 0, 0};

#define MANUAL_LIFT_SPEED_UP	127
#define MANUAL_LIFT_SPEED_DOWN	30

#define LIFT_TIMEOUT			2500 / 26.2

// Experimentally, 106 / 32 worked well for the gain. [dcl]
#define LIFT_GAIN	3
	//mDivideBy32(106)
	//mDivideBy64(Get_Analog_Value(mUserPot1Port))		// max=32

/*
LIFT POT SCALE
 174 at bottom, 1.25"
 472 at midway, 44 3/4 + 1/16 = 44.8125"
 43.5625", 298 units
 298 / 43.5625 * 12
*/
#define LIFT_UNITS_PER_FOOT		93 // 82.089 * 2.5/2.2

#define LIFT_BURN_TIME			200 / 26.2
#define LIFT_COOL_TIME			1000 / 26.2
#define LIFT_STALL_THRESHOLD	8.2

static void doClosedLoopLift(void);
static void liftPresettingBtns(void);

int positions[4] = {
	20,
	(int)(LIFT_UNITS_PER_FOOT * 2),
	(int)((LIFT_UNITS_PER_FOOT * 2) + 34/12 * LIFT_UNITS_PER_FOOT),
	(int)((LIFT_UNITS_PER_FOOT * 2) + 34*2/12 * LIFT_UNITS_PER_FOOT)
};

// ************************************************************************************************

void doLiftBtns(void)
{
	sLift.PWM = 0;
	
	if (mBtnSetLiftPreset) {
		liftPresettingBtns();
	} else {
		// Handle auto lift buttons first so that manual buttons can override
		if (mBtnLiftL0) {		// only in manual mode. Ready Pickup uses same button in actions.c
			moveLiftToLevel(0);
			setFingerPos(G_OPEN);
			setArmPos(G_DOWN);
			setGripperTrap(G_ON);
		}
		if (mBtnLiftL1) {		// all modes.
			moveLiftToLevel(1);
			setArmPos(G_UP);
			setGripperTrap(G_OFF);
		}
		if (mBtnLiftL2) {		// all modes.
			moveLiftToLevel(2);
			setArmPos(G_UP);
			setGripperTrap(G_OFF);
		}
		if (mBtnLiftL3) {		// all modes.
			moveLiftToLevel(3);
			setArmPos(G_UP);
			setGripperTrap(G_OFF);
		}
	}
	
	// Handle manual lift buttons
	if (mBtnLiftUp)
		sLift.PWM = MANUAL_LIFT_SPEED_UP;
	if (mBtnLiftDown)
		sLift.PWM = -MANUAL_LIFT_SPEED_DOWN;
	if (mBtnLiftUp || mBtnLiftDown) {
		sLift.timeout = 0;		// disable closed loop control until user requests it again
	}
	
	if (mBtnPotLift) {
		int potVal = ((int) Get_Analog_Value(mUserPot1Port)) - 512;
		sLift.PWM = mDivideBy4(potVal);
		printf("Pot Lift: %d\r\n", sLift.PWM);
	}
}

// Allow user to set height presets by pressing mBtnSetLiftPreset + mBtnLiftL0/L1/L2/L3
static void liftPresettingBtns(void) {
	static char liftBtnDebouncer = 0;
	int liftpos = getLiftPos();
	
	if (!mBtnSetLiftPreset) return;
	
	if (mBtnLiftL0 && !liftBtnDebouncer) {
		EEPROM_write(kEPROMAdr_LiftL0, ((char*) &liftpos)[0]);		//save across resets
		EEPROM_write(kEPROMAdr_LiftL0+1, ((char*) &liftpos)[1]);	//save across resets
		positions[0] = liftpos;
		printf("Reset Level 0 to %d\r\n", liftpos);
	}
	if (mBtnLiftL1 && !liftBtnDebouncer) {
		EEPROM_write(kEPROMAdr_LiftL1, ((char*) &liftpos)[0]);		//save across resets
		EEPROM_write(kEPROMAdr_LiftL1+1, ((char*) &liftpos)[1]);	//save across resets
		positions[1] = liftpos;
		printf("Reset Level 1 to %d\r\n", liftpos);
	}
	if (mBtnLiftL2 && !liftBtnDebouncer) {
		EEPROM_write(kEPROMAdr_LiftL2, ((char*) &liftpos)[0]);		//save across resets
		EEPROM_write(kEPROMAdr_LiftL2+1, ((char*) &liftpos)[1]);	//save across resets
		positions[2] = liftpos;
		printf("Reset Level 2 to %d\r\n", liftpos);
	}
	if (mBtnLiftL3 && !liftBtnDebouncer) {
		EEPROM_write(kEPROMAdr_LiftL3, ((char*) &liftpos)[0]);		//save across resets
		EEPROM_write(kEPROMAdr_LiftL3+1, ((char*) &liftpos)[1]);	//save across resets
		positions[3] = liftpos;
		printf("Reset Level 3 to %d\r\n", liftpos);
	}
	
	if (mBtnLiftL0 || mBtnLiftL1 || mBtnLiftL2 || mBtnLiftL3)
		liftBtnDebouncer = 1;
	else
		liftBtnDebouncer = 0;
	
	printf("Current Position %d\r\n", liftpos);
	if (user_display_mode)
		gUserByteOI = liftpos;
}

void ReadLiftPresetsFromEPROM(void) {
	( (char*) &(positions[0]) )[0] = EEPROM_read(kEPROMAdr_LiftL0);
	( (char*) &(positions[0]) )[1] = EEPROM_read(kEPROMAdr_LiftL0+1);
	( (char*) &(positions[1]) )[0] = EEPROM_read(kEPROMAdr_LiftL1);
	( (char*) &(positions[1]) )[1] = EEPROM_read(kEPROMAdr_LiftL1+1);
	( (char*) &(positions[2]) )[0] = EEPROM_read(kEPROMAdr_LiftL2);
	( (char*) &(positions[2]) )[1] = EEPROM_read(kEPROMAdr_LiftL2+1);
	( (char*) &(positions[3]) )[0] = EEPROM_read(kEPROMAdr_LiftL3);
	( (char*) &(positions[3]) )[1] = EEPROM_read(kEPROMAdr_LiftL3+1);
}

static void printPresets(void) {
	static char printed = 0;
	if (printed) return;
	printf("READ: Levels %d, %d, %d, %d\r\n", positions[0], positions[1], positions[2], positions[3]);
	printed = 1;
}

void doLift(void) {
	sLift.pos = Get_Analog_Value(mLiftPotPort);		// 0-1023
	
	printPresets();

//	if (sLift.basePos == -1)
//		calibrateLift();

	if (gLoop.onSecondA) {
		printf("Raw Lift Position: %d\r\n", sLift.pos);
	}

	if (mSwLimitLiftLow) {
		sLift.basePos = sLift.pos;		// record base position when lower limit switch
		if (gLoop.onSecondA)
			printf("Lift: Recalibrated, base pos: %d\r\n", sLift.pos);
	}
	
	doClosedLoopLift();

	////////////////////////////////////////////
	////////////// LIFT MOTOR STALL PROTECTION /
	mSLEDLiftStalled = 0;
	mSLEDLiftCooling = 0;
	if (sLift.PWM) {															// If the motor is being powered ...
		if (mAbsDiff(sLift.pos, sLift.stallPos) < LIFT_STALL_THRESHOLD) {		// ... and has not moved a total of STALL_THRESHOLD units ...
			if (sLift.stallTime++ > LIFT_BURN_TIME) {							// ... for BURN_TIME cycles ...
				printf("Lift: Motor on fire! Motor on fire! Disabled.\r\n");	// ... Motor Is On Fire!!!
				sLift.PWM = 0;													// Disable it, quick!
				mSLEDLiftStalled = 1;
			}
		} else {																// If the motor has moved at least STALL_THRESHOLD ...
			sLift.stallTime = 0;												// The motor is not stalled.
			sLift.stallPos = sLift.pos;											// Update the last known position.
		}
	}
	
	if (sLift.PWM == 0 && sLift.stallTime) {				// If the motor is off after being stalled...
		mSLEDLiftCooling = 1;
		if (sLift.coolTime++ > LIFT_COOL_TIME) {			// ... for COOL_TIME cycles ...
			printf("Lift: Motor cooled.\r\n");				// ... the motor is ice cold.
			sLift.stallTime = 0;		// Motor is no longer stalling; we can apply power now.
			sLift.coolTime = 0;
			mSLEDLiftStalled = 0;
			mSLEDLiftCooling = 0;
		}
	}
	
	if (sLift.PWM)
		sLift.coolTime = 0;			// If power is being applied, the motor is not cooling.
	////////////////////////////////////////////

	if (mSwLimitLiftHigh && sLift.PWM > 0) sLift.PWM = 0;
	if (mSwLimitLiftLow && sLift.PWM < 0) sLift.PWM = 0;
	
	mPWMLift = Limit127(sLift.PWM) + 127;
	if (mPWMLift != 127)
		printf("Applying %d to lift\r\n", mPWMLift - 127);
}

// ************************************************************************************************

int moveLiftToTarget(int to) {
	sLift.target = to + sLift.basePos;
	sLift.timeout = LIFT_TIMEOUT;
}

// ************************************************************************************************

int getLiftPos(void) {
	return sLift.pos - sLift.basePos;
}

// ************************************************************************************************

void calibrateLift(void) {
	sLift.target = 0;
	sLift.timeout = LIFT_TIMEOUT;
}

// ************************************************************************************************

void moveLiftToLevel(char level) {
	moveLiftToTarget(positions[level]);
}

// ************************************************************************************************

static void doClosedLoopLift(void)
{
	int error;

	if (sLift.timeout > 0)
		sLift.timeout--;
	else
		return;
	
	error = sLift.pos - sLift.target;		// [-1023, 1023]

	sLift.PWM = -error * LIFT_GAIN;

	if (gLoop.onSecondA)
		printf("doClosedLoopLift: Gain %d; Error: %d, Output: %d\r\n", LIFT_GAIN, error, sLift.PWM);

#define LIFT_MINPWM_UP		80
	if (sLift.PWM > 0 && sLift.PWM < LIFT_MINPWM_UP)
		sLift.PWM = LIFT_MINPWM_UP;
	// No min down

	/*if (mAbsolute(error) < LIFT_MAX_ERROR) {
		sLift.target = -1;
	}*/
}
